#include "Renderer.h"
#include "OpenCLGL_Util.h"
#include <thread>
#include "CostFunction.h"
#include "ParticleSwarm.h"
#include "HandDetector3D.h"

GLFWwindow * mWindow;

using namespace cv;
using namespace std;

//C:\Users\HoangQC\Desktop\TestCase-1\Result

void main()
{
	Mat obsColor = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_color_2.bmp",CV_LOAD_IMAGE_UNCHANGED);
	Mat obsDepth = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\test_depth_2.tiff",CV_LOAD_IMAGE_ANYDEPTH);
	Mat rawColor = imread("C:\\Users\\HoangQC\\Desktop\\TestCase-1\\Result\\color_2.bmp",CV_LOAD_IMAGE_UNCHANGED);
	//waitKey();
	/*Point3i(304,266,957); = -28.8,-46.8,957*/
	//310-304
	HandDetector3D detector(mWindow);
	//detector.runTest();

	detector.setObservation(obsDepth,obsColor,Point3f(-30.f, -46.8f, 954));
	detector.trackingFirstFrame();

	cv::Mat image(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	cv::flip(image, image, 0);
	detector.previewHandImage = image.clone();
	cv::imshow("Show Image", detector.previewHandImage);
	cv::waitKey();

	//circle(detector.previewHandImage,Point2i(64,64),3,Scalar(0,0,0),3);
	detector.drawResultToMat(rawColor,cv::Rect(304-64,266-64,128,128));
	
	cv::imshow("F.U.K",rawColor);
	cv::waitKey();

	/*NEXT STEPS*/
	//detector.setObservation(obsDepth2,obsColor2,Point3d(detector.lastestPos[0], detector.lastestPos[1], detector.lastestPos[2]));
	//detector.trackingNextFrame();
	//image = Mat(128, 128, CV_8UC3, detector.renderer.previewBuffer);
	//cv::flip(image, image, 0);
	//detector.previewHandImage = image.clone();
	//cv::imshow("Show Image", detector.previewHandImage);
	//cv::waitKey();

	//circle(detector.previewHandImage,Point2i(64,64),3,Scalar(0,0,0),3);
	//detector.drawResultToMat(rawColor2,cv::Rect(304-64,266-64,128,128));

	//cv::imshow("F.U.K.2",rawColor2);
	//cv::waitKey();
}